// /////////////////////////////////////////////////////////////////////////////
//
// Name:            aia.cpp
// Author:          Michael Bartsch
//
// Desc :           The program that runs the Artificial Intelligent Agent.
//
// License:         Copyright (C) 2008 Michael Bartsch
//
//                  This program is free software: you can redistribute it
//                  and/or modify it under the terms of the GNU General Public
//                  License as published by the Free Software Foundation,
//                  either version 3 of the License, or (at your option) any
//                  later version.
//
//                  This program is distributed in the hope that it will be
//                  useful, but WITHOUT ANY WARRANTY; without even the implied
//                  warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
//                  PURPOSE. See the GNU General Public License for more
//                  details.
//
//                  You should have received a copy of the GNU General Public
//                  License along with this program. If not, see
//                  <http://www.gnu.org/licenses/>.
//
// /////////////////////////////////////////////////////////////////////////////

// Include files.
#include <stdio.h>

/**
  *  Moves the Aia in the given direction to the given distance.
  *
  *  @param direction direction to ride to
  *  @param distance distance to ride
  */
void motor(char direction, int distance)
{ 
   if(direction == 'f')
   {
      for(int i = 0; i < distance; i += 25)
      {
         analogWrite(6, 255);
         analogWrite(10, 255);
         delay(350);
         analogWrite(6, 0);
         analogWrite(10, 0);
         delay(600);
      }
   }
   
   else if(direction == 'b')
   {
      for(int i = 0; i < distance; i += 25)
      {
         analogWrite(5, 255);
         analogWrite(9, 255);
         delay(350);
         analogWrite(5, 0);
         analogWrite(9, 0);
         delay(600);
      }
   }
}

/**
  *  Rotates the Aai in the given direction with the given degrees.
  *
  *  @param direction direction to turn to
  *  @param degrees to turn
  */
void rotate(char direction, int degrees)
{
   if(direction == 'l')
   {
      for(int i = 0; i < degrees; i += 90)
      {
         analogWrite(5, 255);
         analogWrite(10, 255);
         delay(400);
         analogWrite(5, 0);
         analogWrite(10, 0);
         delay(600);
      }
   }
   
   else if(direction == 'r')
   {
      Serial.print(degrees, DEC);
      for(int i = 0; i < degrees; i += 90)
      {
         analogWrite(6, 255);
         analogWrite(9, 255);
         delay(650);
         analogWrite(6, 0);
         analogWrite(9, 0);
         delay(600);
      }
   }
}

/**
  *  Retrieve data from the sonar.
  */
void sonar()
{
   // Forward declarations.
   unsigned long elapsed = 0;
   char serialBuffer[64]; 
  
   // Send pulse to the sonar to activate a burst.
   digitalWrite(11, LOW);
   delayMicroseconds(2);
   digitalWrite(11, HIGH);
   delayMicroseconds(25);
   digitalWrite(11, LOW);
   
   // Retrieve data.
   elapsed = pulseIn(12, HIGH, 30000);
   
   // Output.
   memset(serialBuffer, 0, 64); 
   sprintf(serialBuffer, "<in sonar %d>", elapsed/58);
   Serial.println(serialBuffer);
}

/**
  *  Executes the given command.
  *
  *  @param command the command to be executed
  *
  *  @return true on succesful execution of command, otherwise false
  */
bool executeCommand(const char* command)
{
   // Check for the namespace.
   // In 
   if(strncmp(command+1, "in", 2) == 0)
   {
      // Check for the module.
      // Motor
      if(strncmp(command+4, "motor", 5) == 0)
      {
         // Forward declarations.
         char direction = 0;
         int distance = 0;
        
         // Determine the direction.
         if(strncmp(command+10, "-f", 2) == 0)
           direction =  'f';

         else if(strncmp(command+10, "-b", 2) == 0)
           direction =  'b';
         
         else return false;   // Error
         
         // Determine the distance.
         char buffer[3];
         memset(buffer, 0, 3);
         for(int i = 0; i < 3; i++)
         {
            if(command[13+i] >= '0' && command[13+i] <= '9')
              buffer[i] = command[13+i];
              
            else if(command[13+i] == '>')
              break;

            else return false;   // Error
         }

         distance = atoi(buffer);
            
         // Execute the module.
         motor(direction, distance);
         return true;
      }
        
      // Rotate
      else if(strncmp(command+4, "rotate", 6) == 0)
      {
         // Forward declarations.
         char direction = 0;
         int degrees = 0;
        
         // Determine the direction
         if(strncmp(command+11, "-l", 2) == 0)
           direction = 'l';

         else if(strncmp(command+11, "-r", 2) == 0)
           direction = 'r';
           
         else return false;   // Error
         
         // Determine the degrees.
         char buffer[3];
         memset(buffer, 0, 3);
         for(int i = 0; i < 3; i++)
         {
            if(command[14+i] >= '0' && command[14+i] <= '9')
              buffer[i] = command[14+i];
              
            else if(command[14+i] == '>')
              break;

            else return false;   // Error
         }

         degrees = atoi(buffer);
            
         // Execute the module.
         rotate(direction, degrees);
         return true;
      }
      
      // Delay
      else if(strncmp(command+4, "delay", 5) == 0)
      {
         // Forward declarations.
         int microseconds = 0;
         
         // Determine the value.
         char buffer[4];
         memset(buffer, 0, 4);
         for(int i = 0; i < 4; i++)
         {
            if(command[10+i] >= '0' && command[10+i] <= '9')
              buffer[i] = command[10+i];
              
            else if(command[10+i] == '>')
              break;

            else return false;   // Error
         }

         microseconds = atoi(buffer);
         
         // Execute the module.
         delay(microseconds);
      }
   }
     
   // Out
   else if(strncmp(command+1, "out", 3) == 0)
   {
      // Check for the module.
      // Sonar
      if(strncmp(command+5, "sonar", 5) == 0)
        sonar();
        
      // Done
      else if(strncmp(command+5, "done", 4) == 0)
        Serial.println("<in done>");
   }
   
   else if(strncmp(command+1, "hello", 5) == 0)
      Serial.println("all your base are belong to us\n");
   
   return false;
}

/**
  *  Initialise the Arduino on start-up.
  */
void setup()
{
   Serial.begin(9600);
   
   pinMode(11, OUTPUT);
   pinMode(12, INPUT);
}

//! Loop to run throught while the Arduino is acti

/**
  *  Loop to run through while the Arduino is active.
  */
void loop()
{
   // Set variables.
   char serialBuffer[64];
   char serialPosition;
   bool isSerialCommand;
   
   // Read from Serial.
   if(Serial.available())
   {
      if(Serial.read() == '<')
      {
         memset(serialBuffer, 0, 64);
         serialBuffer[0] = '<';
         serialPosition = 1;
         isSerialCommand = false;
         
         while(true)
         {
            if(Serial.available() && serialPosition < 64)
            { 
               serialBuffer[serialPosition] = Serial.read();
            
               if(serialBuffer[serialPosition] == '>')
               {
                 isSerialCommand = true;
                 break;
               }
              
               serialPosition++;
            }
            
            else if(serialPosition >= 64)
            {
               isSerialCommand = false;
               break;
            }
         }
      }
   }
   
   if(isSerialCommand)
   {
      executeCommand(serialBuffer);
      isSerialCommand = false;
   }
}

// End of File
